/*
 * Can.cpp
 *
 *  Created on: 5 nov. 2010
 *      Author: eric
 */

#include <util/delay.h>
#include "Can.h"

extern SPI Spi;

byte Can::getStatus(void) {
	byte eflg_reg = readRegister(EFLG);
	byte status = 0;

	if (init_done)
		status = 1;
	if ((eflg_reg & RX1OVR) || (eflg_reg & RX0OVR))
		status += (1 << 3);
	if ((eflg_reg & TXEP) || (eflg_reg & RXEP))
		status += (1 << 2);
	if (tx_nospace)
		status += (1 << 4);
	if (eflg_reg & TXB0)
		status += (1 << 7);

	return status;
}

byte Can::init() {
	// assign CS pin and set it as an output
	pinMode(CS_PIN, OUTPUT);
	digitalWrite(CS_PIN, HIGH);

	// _init_done bit
	init_done = false;

	// txor bit
	tx_nospace = false;

	reset(); // reset device state and volatile memory

	// set digital inputs & outputs
	//bitMod(BFPCTRL, 0x03f, 0x0c); // pins RX0BF and RX1BF as digital outputs
	bitMod(TXRTSCTRL, TXRTSCTRL_B0RTSM + TXRTSCTRL_B1RTSM + TXRTSCTRL_B2RTSM,
			0x00); // pins TX0RTS, TX1RTS and TX2RTS digital inputs

	// set baudrate registers and interrupt mask
	digitalWrite(CS_PIN, LOW); // call Can for SPI link

	Spi.transfer(CAN_SPI_WRITE); // send operation code to Can
	Spi.transfer(CNF3); // point to CNF3 register
	Spi.transfer(mntCNF3); // send CNF3 value
	Spi.transfer(mntCNF2); // send CNF2 value
	Spi.transfer(mntCNF1[B125K]); // send CNF1 value
	Spi.transfer(CANINTE_RX0IE + CANINTE_RX1IE); // send CANINTE value
	Spi.transfer(0x00); // send CANINTF value
	digitalWrite(CS_PIN, HIGH); // close SPI link with Can
	_delay_us(1);

	writeRegister(BFPCTRL, BFPCTRL_B0BFM + BFPCTRL_B1BFM + BFPCTRL_B0BFE
			+ BFPCTRL_B1BFE);

	// _init_done bit
	init_done = true;

	return 1;
}
boolean Can::checkMessage(void) {
	return (digitalRead(CAN_INT));
}

boolean Can::sendMessage(CAN_MSG *pTransmitBuf) {
	byte txbuffer, rts;
	byte i, n;
	byte status = readStatus();

	// check if there is still an available buffer
	tx_nospace = true;
	if (bit_is_clear(status, 2)) {
		tx_nospace = false;
		txbuffer = CAN_SPI_LOAD_TXB0SIDH;
		rts = CAN_RTS_TXB0;
	} else if (bit_is_clear(status, 4)) {
		tx_nospace = false;
		txbuffer = CAN_SPI_LOAD_TXB1SIDH;
		rts = CAN_RTS_TXB1;
	} else if (bit_is_clear(status, 6)) {
		tx_nospace = false;
		txbuffer = CAN_SPI_LOAD_TXB2SIDH;
		rts = CAN_RTS_TXB2;
	} else {
		return false;
	}

	Can::uint32_b4 canID;
	canID.uID = pTransmitBuf->ID;
	// process message header
	byte4 canid = setSID(pTransmitBuf->ID);
	n = (pTransmitBuf->nbytes) & 0x0f;
	if (pTransmitBuf->rtr)
		n += (1 << 6);

	// fill transmit queue
	digitalWrite(CS_PIN, LOW); // call Can for SPI link
	Spi.transfer(txbuffer);
	byte a = Spi.transfer(canid.i16Hi.byteHi);
	a = Spi.transfer(canid.i16Hi.byteLo);
	a = Spi.transfer(canid.i16Lo.byteHi);
	a = Spi.transfer(canid.i16Lo.byteLo);
	Spi.transfer(n, 10);

	n &= 0x0f;
	for (i = 0; i < n; i++)
		Spi.transfer(pTransmitBuf->data[i]); // send data bytes

	digitalWrite(CS_PIN, HIGH); // stop SPI link

	// set transmit request to Can
	digitalWrite(CS_PIN, LOW); // call Can for SPI link
	Spi.transfer(rts); // send request
	digitalWrite(CS_PIN, HIGH); // stop SPI link

	return true;
}

boolean Can::readMessage(CAN_MSG *pTransmitBuf) {
	byte status = readRxStatus();
	byte rxbuffer;
	byte i, n;
	byte4 canid;

	if (bit_is_set(status, 6)) {
		rxbuffer = CAN_SPI_READ_RXB0SIDH;
	} else if (bit_is_set(status, 7)) {
		rxbuffer = CAN_SPI_READ_RXB1SIDH;
	} else {
		return false;
	}
	// Get receive buffer
	digitalWrite(CS_PIN, LOW); // call Can for SPI link
	Spi.transfer(rxbuffer);

	// Get message ID
	canid.i16Hi.byteHi = Spi.transfer(0x00);
	canid.i16Hi.byteLo = Spi.transfer(0x00);
	canid.i16Lo.byteHi = Spi.transfer(0x00);
	canid.i16Lo.byteLo = Spi.transfer(0x00);

	pTransmitBuf->ID = getSID(canid);
	Can::uint32_b4 canID;
	canID.uID = pTransmitBuf->ID;
	// Get message length and rtr bit
	pTransmitBuf->nbytes = (Spi.transfer(0x00) & 0x0f);
	pTransmitBuf->rtr = (bit_is_set(status, 3)) ? 1 : 0;
	n = pTransmitBuf->nbytes;

	// Get message data
	for (i = 0; i < n; i++)
		pTransmitBuf->data[i] = Spi.transfer(0x00);

	digitalWrite(CS_PIN, HIGH); // stop SPI link

	// Reset interrupt flag
	if (bit_is_set(status, 6)) {
		bitMod(CANINTE, CANINTE_RX0IE, 0);
	} else {
		bitMod(CANINTE, CANINTE_RX1IE, 0);
	}
	// quit routine
	return true;
}

boolean Can::setMode(byte _mode) {
	if ((_mode > 4) || (_mode < 0))
		return false;

	byte dummy = _mode << 5; // shift left mode byte 5 bits
	bitMod(CANCTRL, 0xe0, dummy); // write REQOP<2..0> bits
	delay(1);

	return true;
}

void Can::reset(void) {
	digitalWrite(CS_PIN, LOW); // call Can for SPI link
	Spi.transfer(CAN_SPI_RESET); // send operation code to Can
	_delay_us(5);
	digitalWrite(CS_PIN, HIGH); // close SPI link with Can
	_delay_us(1);
}

void Can::writeRegister(byte RegAdd, byte RegValue) {
	digitalWrite(CS_PIN, LOW); // call Can for SPI link
	Spi.transfer(CAN_SPI_WRITE); // send operation code to Can
	Spi.transfer(RegAdd); // send address to write
	Spi.transfer(RegValue); // send value
	digitalWrite(CS_PIN, HIGH); // close SPI link with Can
}
byte Can::readRegister(byte RegAdd) {
	digitalWrite(CS_PIN, LOW); // call Can for SPI link
	Spi.transfer(CAN_SPI_READ); // send operation code to Can
	Spi.transfer(RegAdd); // send address to read
	byte dummy = Spi.transfer(0xa5); // send value
	digitalWrite(CS_PIN, HIGH); // close SPI link with Can

	return dummy;
}
byte Can::readStatus() {
	digitalWrite(CS_PIN, LOW); // call Can for SPI link
	Spi.transfer(CAN_SPI_READ_STATUS); // send operation code to Can
	byte dummy = Spi.transfer(0xa5); // send value
	Spi.transfer(0xa5); // read value twice
	digitalWrite(CS_PIN, HIGH); // close SPI link with Can

	return dummy;
}
byte Can::readRxStatus() {
	digitalWrite(CS_PIN, LOW); // call Can for SPI link
	Spi.transfer(CAN_SPI_RX_STATUS); // send operation code to Can
	byte dummy = Spi.transfer(0xa5); // read value
	Spi.transfer(0xa5); // read value twice
	digitalWrite(CS_PIN, HIGH); // close SPI link with Can

	return dummy;
}
void Can::bitMod(byte RegAdd, byte RegMask, byte RegValue) {
	digitalWrite(CS_PIN, LOW); // call Can for SPI link
	Spi.transfer(CAN_SPI_BIT_MODIFY); // send operation code to Can
	Spi.transfer(RegAdd); // send address to read
	Spi.transfer(RegMask); // send bit-mask
	Spi.transfer(RegValue); // send value
	digitalWrite(CS_PIN, HIGH); // close SPI link with Can
}

Can::byte4 Can::setSID(uint32_t ID) {
	uint16_2 DoubleInt16 = move32to16(ID);
	byte4 FourByte;
	byte tmp;

	FourByte.i16Hi = move16to8((int) DoubleInt16.i16Hi << 3);
	tmp = FourByte.i16Hi.byteLo;
	FourByte.i16Hi.byteLo &= 0xe0;
	FourByte.i16Hi.byteLo |= (1 << EXIDE);
	FourByte.i16Hi.byteLo |= (tmp >> 3) & 0x03;
	FourByte.i16Lo = move16to8((int) DoubleInt16.i16Lo);
	return FourByte;
}

uint32_t Can::getSID(Can::byte4 DoubleInt16) {
	uint16_t intHi = move8to16((byte2) DoubleInt16.i16Hi);
	uint16_t intLo = move8to16((byte2) DoubleInt16.i16Lo);

	intHi = ((intHi >> 3) & 0x1FFC) | (intHi & 0x0003);
	//uint32_t tmp = ((uint32_t)intHi << 16)  + intLo;
	return (((uint32_t) intHi << 16) + intLo);
}

Can::byte2 Can::move16to8(uint16_t Value) {
	byte2 DoubleByte;

	DoubleByte.byteLo = (byte) (Value & 0x00ff);
	DoubleByte.byteHi = (byte) (Value >> 8);

	return DoubleByte;
}

Can::uint16_2 Can::move32to16(uint32_t Value) {
	uint16_2 DoubleInt16;

	DoubleInt16.i16Lo = (uint16_t) (Value & 0x0000ffff);
	DoubleInt16.i16Hi = (uint16_t) (Value >> 16);

	return DoubleInt16;
}

uint16_t Can::move8to16(byte2 DoubleByte) {
	uint16_t Value;
	Value = ((uint16_t) (DoubleByte.byteHi)) << 8;
	Value += (uint16_t) (DoubleByte.byteLo);

	return Value;
}

